#include "pic24_all.h"
// Copyright (C) 2010-2011 Hari N. Nair, 2011 Techrhythm Inc 

// This program is free software; you can redistribute it and/or
// modify it under the terms of the GNU General Public License
// as published by the Free Software Foundation; either version 2
// of the License, or (at your option) any later version.

// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
// GNU General Public License for more details.

// You should have received a copy of the GNU General Public License
// along with this program; if not, write to the Free Software
// Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.





// Low power FRC clock 500kHz. CLKDIV set to 0 => Divide by 1 => output clock is 500kHz. 
// MCU instruction clock is 500kHz/2 = 250kHz
// Brown-out reset voltage is set to 1.8V

_FBS( BSS_OFF & BWRP_OFF )
_FGS( GSS0_OFF & GWRP_OFF )
_FOSCSEL( FNOSC_LPFRCDIV & IESO_OFF )
_FOSC( FCKSM_CSECMD & POSCFREQ_100KHZ & OSCIOFNC_ON & POSCMOD_NONE)
_FWDT( FWDTEN_OFF & WINDIS_OFF & FWPSA_PR32 & WDTPS_PS1 )
_FPOR( MCLRE_ON & BORV_1_8V & AI2C1SEL_PRI & PWRTEN_OFF & BOREN_BOR3)
_FICD( BKBUG_OFF & COE_OFF & ICS_PGx3 )
_FDS( DSWDTEN_OFF & DSBOR_OFF & RTCCREF_OFF & DSWDTLPRC_OFF & DSWDTPS_DSWDTPS_0 )


// if DEBUG is enabled, the UART is enabled and debug messages are printed to the UART terminal
// using UART2 TX pin (this does double duty with the PGD pin). The messages can be displayed using
// the Microchip Pickit2 UART window, in the drop-down baudrate selection, select "custom baudrate"
// and set it to 8928.

// The debug messages include the sensor calibration coefficients and the cruise altitude and pressure.

//#define DEBUG

#ifdef DEBUG
   #include <stdio.h>
#endif

#define MAX_Z_SAMPLES                 85

// Sampling frequency is 26 temperature compensated pressure readings per second
#define SENSOR_SAMPLES_PER_SEC        26

// These timer delays give us 26 samples/second with FCY 250kHz, 
// taking into account spi write and read delays
#define SENSOR_TEMPERATURE_TIMEOUT      125
#define SENSOR_PRESSURE_TIMEOUT         185

// pressure sensor sampling states
#define SENSOR_READ_TEMPERATURE 		11
#define SENSOR_READ_PRESSURE			22

// Beyond this climbrate (cm per sec), the audio tone does not change, even though the
// climbrate computation is valid to +/- 20m/s
#define VARIO_MAX_CPS         1000L

// more audio discrimination for climbrates under this threshold
#define VARIO_XOVER_CPS       300L

// change these parameters based on the frequency bandwidth of the piezo transducer
// this is good for the PUI Audio piezo

#define VARIO_MAX_FREQHZ      1100L
#define VARIO_XOVER_FREQHZ    600L
#define VARIO_MIN_FREQHZ      300L

#define VARIO_SINK_FREQHZ     400L
#define VARIO_TICK_FREQHZ     300L

#define ACCEL_MIN_ADAPT     5
#define ACCEL_MAX_ADAPT     15

#define KLPF_MAX_ADAPT			232
#define KLPF_MIN_ADAPT			 0
#define KLPF_DEFAULT_ADAPT		160

#define KLPF_ACCEL   			200L
#define KLPF_CLIMBRATE   		161L

#define SINKTONE_CPS_DFLT     -250L
#define SINKTONE_CPS_MIN      -400L
#define SINKTONE_CPS_MAX      -100L

#define CLIMBTONE_CPS_MAX     50L
#define CLIMBTONE_CPS_DFLT    10L
#define CLIMBTONE_CPS_MIN     0L  

#define LIFTYAIRTONE_CPS_MAX     30L
#define LIFTYAIRTONE_CPS_DFLT    -30L
#define LIFTYAIRTONE_CPS_MIN     -130L


// With a new 3V CR2450 battery, circuit current drain in operating mode is < 1mA.
// In sleep mode, current drain < 10uA is possible  with the use of good quality 
// SMD ceramic capacitors (low leakage current)

// sw will automatically put the mcu in sleep mode to conserve battery power
// if no significant vertical movement (> +/- 30cm/s) is detected in a 30 minute period 
#define SLEEP_TIMEOUT_MINUTES             30
#define SLEEP_THRESHOLD_CPS				  30


#define TONE_LO_FREQHZ  300L
#define TONE_MID_FREQHZ 600L
#define TONE_HI_FREQHZ	1000L


#define FLAG_FILTER_MODE_ADAPTIVE       0x01

#define VARIO_STATE_SINK    	11
#define VARIO_STATE_QUIET   	22
#define VARIO_STATE_LIFTY_AIR	33
#define VARIO_STATE_CLIMB   	44

int gFlags;

// gpioDbg is connected to pin 6 of the pickit2 connector, debug signals on this pin
// are observed using the Pickit2 Logic Analyzer window to profile the
// pressure sampling and computation timing to validate that the unit is 
// operating correctly
#define gpioDbg         _LATB15

#define gpioSW1   _RA1  
#define gpioSW2   _RB4
#define gpioSW4   _RB2
#define gpioSW8   _RA2

#define SCK     _LATA4
#define MOSI    _LATB9
#define CS      _LATB7
#define MISO    _RB8

int gnSensorState;
int gnSmpCnt;
int gnNumSamples;


// set of parameters selected by the rotary switch position
typedef struct OPT_ {
    int liftyAirToneCps;
    int climbToneCps;
    int sinkToneCps;
    int kLpfMax;
    int numSamples;
	int discrimThreshold;
    int tableSelect;
} OPT;

// climb audio beep period and "duty cycle"
typedef struct BEEP_ {
    int periodTicks;
    int endTick;
} BEEP;
    

OPT gOptionsTbl[16] = {
    {10,10,-250,161,70,25,0},   // 0
    {-30,10,-250,161,70,25,0},   // 1
    {-90,10,-250,161,70,25,0},   // 2
    {-130,10,-250,161,70,25,0},  // 3

    {20,20,-400,161,70,25,1},   // 4
    {-20,20,-400,161,70,25,1},   // 5
    {-80,20,-400,161,70,25,1},   // 6
    {-120,20,-400,161,70,25,1},  // 7
    
    {10,10,-250,231,85,25,0},   // 8
    {-30,10,-250,231,85,25,0},   // 9
    {-90,10,-250,231,85,25,0},   // A
    {-130,10,-250,231,85,25,0},  // B
    
    {20,20,-400,231,85,25,1},   // C
    {-20,20,-400,231,85,25,1},   // D
    {-80,20,-400,231,85,25,1},   // E
    {-120,20,-400,231,85,25,1}   // F
};    
   
    

int gnOptSelect;  // range 0 .. 15, selects one of the entries in the gOptions table

// Timer, variables used to generate the automatic timeout sleep 
volatile u32 gnTimeTick;
volatile int gnSecTick;
volatile int gnElapsedHrs;
volatile int gnElapsedMin;
volatile int gnElapsedSec;
volatile int gbTmr3Flag;
volatile int gbTmr1Flag;

s32 gZBuf[MAX_Z_SAMPLES];
s32 gnAltAvgM; 
s32 gnAltAvgCm;
s32 gnCps;
s32 gnSum_t;
s32 gnSum_t2;
s32 gnDen;
s32 gnPa;

s32 gnTempC;

s32 gnDiscrimThreshold =  15L;
s16 gnBeepTableSelect = 0;
s16 gnBeepPeriodTicks = 0;
s16 gnBeepEndTick = 0;
s32 gnBeepCps = 0;
s32 gnVarioCps = 0;
s32 gnFreqHz  = 0;

s32 gnSinkToneCps   = SINKTONE_CPS_DFLT;
s32 gnClimbToneCps  = CLIMBTONE_CPS_DFLT;
s32 gnLiftyAirToneCps   = LIFTYAIRTONE_CPS_DFLT;
s32 gnKLpf          = KLPF_DEFAULT_ADAPT;
s32 gnKLpfMax       = KLPF_MAX_ADAPT;
s32 gnAccel = 0;
s32 gnCpsx256 = 0;
s32 gnAccelx256 = 0;

// calibration coefficients
u16 gC[8];
u08 crc;
u32 gD1;
u32 gD2;
s32 gdT;
s32 gTEMP;
s64 gOFF;
s64 gSENS;
s32 gP;

int gnVarioState;

// temp buffer used to print debug messages to the UART. 
// Use the pickit2 UART terminal window to see the messages,
// set the baudrate to Custom, 8928 baud
 
#ifdef DEBUG
   char gszBuf[60];
#endif

void audio_Config(void);
void audio_SetFreq(s32 nFreqHz);
void audio_Beep(s32 nCps);
void audio_Tone(s32 freqHz, int msx10);

void sns_TriggerPressureSample(void);
void sns_TriggerTemperatureSample(void);
u32  sns_ReadPressureSample(void);
u32  sns_ReadTemperatureSample(void);
s32  sns_CalculateTemperatureCx10(void);
s32  sns_CalculatePressurePa(void);
void sns_CalculateSensorNoisePa(void);
void sns_DeltaCmPerSec(void);
void sns_Init(void);
void sns_Reset(void);
void sns_InitData(void);
void sns_InitWindowBuffers(void);
void sns_ReadCoeffs(void);
void sns_SampleContinuous(void);
s32  sns_Pa2Cm(s32 pa);
void sns_AverageAltitude(void);
void sns_LPFClimbRate(void);
void sns_LPFAccel(void);
s32  sns_AdaptKLpf(void);

void mcu_Init(void);
static inline void mcu_Sleep();
void mcu_PeriphConfig(void);
void mcu_PeriphDisable(void);
int sns_CheckOptSelect(void);

void cmp_Config(void);
void tmr1_Config(void);
void tmr1_BusyWaitMsx10(u32 mSx10);
void tmr1_Start(void);
int  tmr1_ElapsedTimeMs(void);
void tmr1_FlagWaitMsx10(u32 mSx10);
void tmr2_Config(void);
void tmr3_Config(void);

#ifdef DEBUG
   void uart_Config(void);
   void uart_Print(char *szBuf);
#endif 

void spi_Config(void);
u32 spi_Receive24(void);
u16 spi_Receive16(void);
void spi_Send8(u08 out);

void adc_Config(u16  ch0PositiveMask, u08  autoSampleTime);
u16  adc_Convert(void);
void bat_VoltageCheck(void);

const char szCopyright[] = "Copyright (c) Hari N. Nair, Bangalore, 2011";
const char szVersion[] = "Ver 1.11";

int main(void) {
   mcu_Init();
   tmr1_BusyWaitMsx10(10000);
   bat_VoltageCheck();
   tmr1_BusyWaitMsx10(10000);     
#ifdef DEBUG
   tmr1_BusyWaitMsx10(20000);
   uart_Print("\r\n");
   uart_Print(szVersion);
   uart_Print("\r\n");
#endif
   
   gnOptSelect = sns_CheckOptSelect();
   sns_Init();
   sns_InitData();
        
   while (1) {
      sns_SampleContinuous();           
	  }
   return 0;
   }


static inline void mcu_Sleep() {
#ifdef DEBUG
   uart_Print("Going to sleep\r\n");
#endif
   tmr1_Config();
   // descending 3-tone alarm to let us know its going to sleep
   audio_Tone(TONE_HI_FREQHZ,10000);
   audio_Tone(TONE_MID_FREQHZ,10000);
   audio_Tone(TONE_LO_FREQHZ,10000);

   gpioDbg = 0;
   mcu_PeriphDisable();
   Nop();Nop();Nop();
   Sleep();
   }


void mcu_Init(void) {
   AD1PCFG = 0xFFFF;            // set all analog-input enabled pins (ANx) to digital mode
   CLKDIVbits.RCDIV = 0;  		// LPFRC oscillator 500kHz divided by 1
   CONFIG_RB15_AS_DIG_INPUT();  
   CONFIG_RA1_AS_DIG_INPUT();  
   ENABLE_RA1_PULLUP();
   CONFIG_RA2_AS_DIG_INPUT();   
   ENABLE_RA2_PULLUP();
   CONFIG_RB2_AS_DIG_INPUT();
   ENABLE_RB2_PULLUP();
   CONFIG_RB4_AS_DIG_INPUT();
   ENABLE_RB4_PULLUP();
   
   CONFIG_RA4_AS_DIG_OUTPUT(); // sclk
   CONFIG_RB7_AS_DIG_OUTPUT(); // cs
   CONFIG_RB8_AS_DIG_INPUT();  // miso
   CONFIG_RB9_AS_DIG_OUTPUT(); // mosi
      
   mcu_PeriphConfig();
   }

void mcu_PeriphConfig(void) {
   spi_Config();
   audio_Config();
   tmr1_Config();
   tmr3_Config();
   cmp_Config();
   gpioDbg = 0;
#ifdef DEBUG
   uart_Config();
#endif
   }

int sns_CheckOptSelect(void) {
    int sel;
    sel = 15;
    if (gpioSW1) sel &= ~0x1;
    if (gpioSW2) sel &= ~0x2;
    if (gpioSW4) sel &= ~0x4;
    if (gpioSW8) sel &= ~0x8;
    return sel;
    }
    
void mcu_PeriphDisable(void) {
   T1CONbits.TON = 0;    // disable tmr1
   _T1IF = 0;            //clear interrupt flag
   _T1IE = 0;            //disable the interrupt
   T2CONbits.TON = 0;    // disable tmr2
   OC1CONbits.OCM = 0; // disable ocm
   _T3IF = 0;            //clear interrupt flag
   _T3IE = 0;            // disable interrupt
   T3CONbits.TON = 0;    // disable tmr3
#ifdef DEBUG
   U2MODEbits.UARTEN = 0; // disable uart2
   // clear IF flags 
   IFS1bits.U2RXIF = 0;
   IFS1bits.U2TXIF = 0;
   // disable interrupt 
   IEC1bits.U2RXIE = 0;
   IEC1bits.U2TXIE = 0;
#endif
   CM1CONbits.COE = 0;  // comparator output disabled
   CM1CONbits.CEN = 0;  // comparator disable

   // config i/o pins as digital inputs to minimize power 
   CONFIG_RA0_AS_DIG_INPUT();
   CONFIG_RA1_AS_DIG_INPUT();
   CONFIG_RA2_AS_DIG_INPUT();
   CONFIG_RA3_AS_DIG_INPUT();
   CONFIG_RA4_AS_DIG_INPUT();
   CONFIG_RA6_AS_DIG_INPUT();  
   CONFIG_RB0_AS_DIG_INPUT();
   CONFIG_RB1_AS_DIG_INPUT();
   CONFIG_RB2_AS_DIG_INPUT();
   CONFIG_RB4_AS_DIG_INPUT();
   CONFIG_RB7_AS_DIG_INPUT();
   CONFIG_RB8_AS_DIG_INPUT();
   CONFIG_RB9_AS_DIG_INPUT();
   CONFIG_RB12_AS_DIG_INPUT();
   CONFIG_RB13_AS_DIG_INPUT();
   CONFIG_RB14_AS_DIG_INPUT();
   CONFIG_RB15_AS_DIG_INPUT();
   }   

void cmp_Config(void) {
   CM1CONbits.COE = 1;  // output goes to pin 17
   CM1CONbits.CPOL = 1; // inverted
   CM1CONbits.CLPWR = 0; // no operation in low power mode
   CM1CONbits.EVPOL = 0; // no interrupt/event
   CM1CONbits.CREF = 0; // + input connected to C1INA (pin 8)
   CM1CONbits.CCH = 3; // - input connected to VBG/2
   CM1CONbits.CEN = 1;  // enable
 
   Nop();Nop();Nop();Nop();Nop();
   _C1EVT = 0; //clear C1 event flag
   _CMIF = 0;  //clear interrupt flag
  }

/// TMR2 used for generating audio tone square wave 
void tmr2_Config(void) {
   T2CONbits.T32 = 0; // 16bit timer
   T2CONbits.TCS = 0; //  select internal clock 
   T2CONbits.TGATE = 0; // gate disabled
   T2CONbits.TSIDL = 0; // continue in idle mode
   T2CONbits.TCKPS1 = 0; // Prescaler 1:8
   T2CONbits.TCKPS0 = 1; // Prescaler 1:8
   T2CONbits.TON = 1; // start tmr2
   }

void audio_Config(void) {
   tmr2_Config();
   OC1CONbits.OCTSEL = 0; // use TMR2
   gnVarioState = VARIO_STATE_QUIET;
   }

void audio_SetFreq(s32 nFreqHz) {
   u16 tmrTicks;
   OC1CONbits.OCM = 0; // disable ocm
   T2CONbits.TON = 0; // stop tmr2
   TMR2 = 0;
   if (nFreqHz) {
      tmrTicks = (u16)((FCY>>3)/nFreqHz);
      OC1R = (tmrTicks >> 1); // square wave
      OC1RS = tmrTicks;
      PR2 = tmrTicks;
      T2CONbits.TON = 1; // start tmr2
      OC1CONbits.OCM = 5; // continuous pulse mode
      }
    else { // dc 0V
      OC1R = 50;
      OC1RS = 100;
      PR2 = 40;
      }
   }

void audio_Tone(s32 freqHz, int msx10) {
   audio_SetFreq(0);
   tmr1_BusyWaitMsx10(100);
   audio_SetFreq(freqHz);
   tmr1_BusyWaitMsx10(msx10);
   audio_SetFreq(0);
   }

const int gnOffScaleHiTone[10]= {2000,1000,2000,1000,2000,1000,2000,1000,2000,1000};
const int gnOffScaleLoTone[10]= {650,600,550,500,450,400,350,300,250,200};
const int gnLiftyAirToneDelta[8] = {-40,-40,-40,-40,-40,-40,-40,-40};
const int gnClimbToneDelta[8] = {0,0,0,0,0,0,0,0};
const int gnSinkToneDelta[20] = {-10,-10,-10,-10,-10,-10,-10,-10,-10,-10,-10,-10,-10,-10,-10,-10,-10,-10,-10,10};


// table for beep duration and repeat rate based on vertical speed
const BEEP gBeepTbl[2][10] = {
// Table 0 : for weak conditions, repetition rate saturates at 5m/s
{{8,2}, 
{6,2}, 
{4,2}, 
{3,1}, 
{3,1}, 
{2,1}, 
{2,1}, 
{2,1}, 
{2,1}, 
{2,1}},

// Table 1 : for strong conditions, repetition rate saturates at 7m/s
{{8,2}, 
{7,2}, 
{6,2}, 
{5,2}, 
{4,2}, 
{3,1}, 
{3,1}, 
{2,1}, 
{2,1}, 
{2,1}},
};




void audio_Beep(s32 nCps) {
   static s32 nFreqHz,nTick;
   int nRange;
   s32 newFreqHz = 0;

   if ((gnBeepPeriodTicks <= 0) || ((gnVarioState != VARIO_STATE_LIFTY_AIR) && (gnVarioState != VARIO_STATE_SINK) && (ABS(nCps-gnVarioCps) > gnDiscrimThreshold) && (ABS(nCps) < VARIO_MAX_CPS)) || ((nCps > gnClimbToneCps) && (gnVarioCps <= gnClimbToneCps)) ) {
      gnVarioCps = nCps;

      // if sinking much faster than glider sink rate, generate continuous tone alarm
      if (gnVarioCps <= gnSinkToneCps) {
         gnVarioState = VARIO_STATE_SINK;
         gnBeepCps = gnVarioCps;	
         if (gnBeepCps < -VARIO_MAX_CPS) {
			gnBeepCps = -VARIO_MAX_CPS;
			}
		 nTick = 0;
		 if (gnBeepCps == -VARIO_MAX_CPS) {
    		gnBeepPeriodTicks = 10;
    		gnBeepEndTick = 10;
    		newFreqHz =gnOffScaleLoTone[0];
    		nFreqHz = newFreqHz;	
            audio_SetFreq(nFreqHz);
            }
         else {
            gnBeepPeriodTicks = 20;
            gnBeepEndTick  = 18;
            newFreqHz = VARIO_SINK_FREQHZ + ((gnSinkToneCps-gnBeepCps)*(VARIO_MAX_FREQHZ - VARIO_SINK_FREQHZ))/(VARIO_MAX_CPS+gnSinkToneCps);
            CLAMP(newFreqHz,VARIO_MIN_FREQHZ,VARIO_MAX_FREQHZ);
            nFreqHz = newFreqHz;
            audio_SetFreq(nFreqHz);
         	}
        }      
      //if climbing, generate beeps
      else {
         if (gnVarioCps >= gnClimbToneCps) {
            gnVarioState = VARIO_STATE_CLIMB;
            gnBeepCps = gnVarioCps;
            if (gnBeepCps > VARIO_MAX_CPS) {
				gnBeepCps = VARIO_MAX_CPS;
				}
		    nTick = 0;
			if (gnBeepCps == VARIO_MAX_CPS) {
    		    gnBeepPeriodTicks = 10;
    		    gnBeepEndTick = 10;
    		    newFreqHz =gnOffScaleHiTone[0];
    		    nFreqHz = newFreqHz;	
                audio_SetFreq(nFreqHz);
                }
            else { 			
           	    nRange = gnBeepCps/100;
           	    if (nRange > 9) nRange = 9;
           	    gnBeepPeriodTicks = gBeepTbl[gnBeepTableSelect][nRange].periodTicks;
           	    gnBeepEndTick = gBeepTbl[gnBeepTableSelect][nRange].endTick;
         	    if (gnBeepCps > VARIO_XOVER_CPS) {
                    newFreqHz = VARIO_XOVER_FREQHZ + ((gnBeepCps - VARIO_XOVER_CPS)*(VARIO_MAX_FREQHZ - VARIO_XOVER_FREQHZ))/(VARIO_MAX_CPS - VARIO_XOVER_CPS);
                    }
                else {
                    newFreqHz = VARIO_MIN_FREQHZ + (gnBeepCps*(VARIO_XOVER_FREQHZ - VARIO_MIN_FREQHZ))/VARIO_XOVER_CPS;
                    }
                CLAMP(newFreqHz,VARIO_MIN_FREQHZ,VARIO_MAX_FREQHZ);   
                nFreqHz = newFreqHz;
                audio_SetFreq(nFreqHz);
                }
            }       
         else   // in "lifty-air" band, indicate with different sound
         if (gnVarioCps >= gnLiftyAirToneCps) {
              gnVarioState = VARIO_STATE_LIFTY_AIR;
    		  gnBeepCps = gnVarioCps;
    		  nTick = 0;
    		  gnBeepPeriodTicks = 24;
    		  gnBeepEndTick = 2;
    		  newFreqHz = VARIO_TICK_FREQHZ + ((gnBeepCps - gnLiftyAirToneCps)*400)/(gnClimbToneCps-gnLiftyAirToneCps);
              CLAMP(newFreqHz,VARIO_MIN_FREQHZ,VARIO_MAX_FREQHZ);   
              nFreqHz = newFreqHz;
    		  audio_SetFreq(nFreqHz);  // higher frequency as you approach climb threshold
			  }

      // not sinking enough to trigger alarm,  be quiet
         else{
            gnVarioState = VARIO_STATE_QUIET;
            nTick = 0;
            gnBeepPeriodTicks = 0;
            gnBeepEndTick  = 0;
            newFreqHz = nFreqHz = 0;
            audio_SetFreq(nFreqHz);
            }
         }
      }
   else{ 
      nTick++;
      gnBeepPeriodTicks--;
      if (nTick >= gnBeepEndTick){ // shut off tone
         newFreqHz = 0;
         }
	  else
	  if (gnBeepCps == VARIO_MAX_CPS) {
    	  newFreqHz = gnOffScaleHiTone[nTick];
            }
      else 	  
	  if (gnBeepCps == -VARIO_MAX_CPS) {
    	  newFreqHz = gnOffScaleLoTone[nTick];
          }
      else
      if (gnVarioState == VARIO_STATE_LIFTY_AIR) {
   		 newFreqHz = nFreqHz + gnLiftyAirToneDelta[nTick];
         }  		
	 else 
	 if (gnVarioState == VARIO_STATE_CLIMB) {
    	 newFreqHz = nFreqHz + gnClimbToneDelta[nTick];
         }
     else  		
	 if (gnVarioState == VARIO_STATE_SINK) {
    	 newFreqHz = nFreqHz + gnSinkToneDelta[nTick];
         }
     if (newFreqHz != nFreqHz) {
         nFreqHz = newFreqHz;
         audio_SetFreq(nFreqHz);
         }    	
	  }
   }



void sns_Init(void) {
   gFlags = (FLAG_FILTER_MODE_ADAPTIVE);  
   gnLiftyAirToneCps = gOptionsTbl[gnOptSelect].liftyAirToneCps;
   gnClimbToneCps = gOptionsTbl[gnOptSelect].climbToneCps;
   gnSinkToneCps = gOptionsTbl[gnOptSelect].sinkToneCps;
   gnKLpfMax = gOptionsTbl[gnOptSelect].kLpfMax;
   gnKLpf = gnKLpfMax;
   gnNumSamples = gOptionsTbl[gnOptSelect].numSamples;
   gnDiscrimThreshold = gOptionsTbl[gnOptSelect].discrimThreshold;
   gnBeepTableSelect = gOptionsTbl[gnOptSelect].tableSelect;
#ifdef DEBUG
    sprintf(gszBuf,"Option Selected = %d\r\n",gnOptSelect);
    uart_Print(gszBuf);
    sprintf(gszBuf,"Lifty Air Threshold = %ld\r\n",gnLiftyAirToneCps);
    uart_Print(gszBuf);
    sprintf(gszBuf,"Climb Threshold = %ld\r\n",gnClimbToneCps);
    uart_Print(gszBuf);
    sprintf(gszBuf,"Sink Threshold = %ld\r\n",gnSinkToneCps);
    uart_Print(gszBuf);
    sprintf(gszBuf,"Number Samples = %d\r\n",gnNumSamples);
    uart_Print(gszBuf);
    sprintf(gszBuf,"Discrimination Threshold = %ld\r\n",gnDiscrimThreshold);
    uart_Print(gszBuf);
    sprintf(gszBuf,"Table Select = %d\r\n",gnBeepTableSelect);
    uart_Print(gszBuf);
#endif       
   
   gnCps = 0;
   gnPa = 0;
   gnAltAvgM = 0;
   gnAltAvgCm = 0;
   gnCpsx256 = 0;
   gnAccelx256 = 0;  
   gnAccel = 0; 
   // following parameters used in climb/sink rate calculation need to be computed only when 
   // the number of window samples changes. 
   gnSum_t = -(gnNumSamples * (gnNumSamples-1L))/2L;
   gnSum_t2 = (gnNumSamples * (gnNumSamples-1L)*(2L*gnNumSamples-1L))/6L;
   gnDen = (gnNumSamples*gnSum_t2) - (gnSum_t*gnSum_t);
   //sns_Reset();
   sns_ReadCoeffs();
   }



void sns_InitData(void) {
#ifdef DEBUG
   sns_CalculateSensorNoisePa();
#endif    
   sns_InitWindowBuffers();
   sns_TriggerTemperatureSample();
   gnSensorState = SENSOR_READ_TEMPERATURE;
   tmr1_FlagWaitMsx10(SENSOR_TEMPERATURE_TIMEOUT);
   }
   

void sns_SampleContinuous(void) {
    int optSelect;
   if (!gbTmr1Flag) return;
   if (gnSensorState == SENSOR_READ_TEMPERATURE) {
      gD2 = sns_ReadTemperatureSample();
      sns_TriggerPressureSample();
      tmr1_FlagWaitMsx10(SENSOR_PRESSURE_TIMEOUT);
#ifdef DEBUG      
      gpioDbg = 1; // turn on the debug pulse for timing the critical computation
#endif      
      gTEMP = sns_CalculateTemperatureCx10();       
      gnTempC = (gTEMP >= 0?  (gTEMP+5)/10 : (gTEMP-5)/10);
      gnPa = sns_CalculatePressurePa();
      gZBuf[gnSmpCnt] = sns_Pa2Cm(gnPa);
      sns_AverageAltitude();
   	  sns_DeltaCmPerSec();
#ifdef DEBUG  	  
      gpioDbg = 0;
#endif      
	  gnSensorState = SENSOR_READ_PRESSURE;
      }
   else
   if (gnSensorState == SENSOR_READ_PRESSURE) {
      gD1 = sns_ReadPressureSample();
      optSelect = sns_CheckOptSelect();
      if (optSelect != gnOptSelect) {
          // new option selected, reset
          audio_Tone(TONE_HI_FREQHZ,100);          
          asm("RESET");
          }
      sns_TriggerTemperatureSample();
      tmr1_FlagWaitMsx10(SENSOR_TEMPERATURE_TIMEOUT);
#ifdef DEBUG
      gpioDbg = 1;
#endif      
  	  gnKLpf = ((gFlags & FLAG_FILTER_MODE_ADAPTIVE) ? sns_AdaptKLpf() : gnKLpfMax);
	  sns_LPFClimbRate();
   	  gnSmpCnt++;
   	  if (gnSmpCnt == gnNumSamples) {
       	 gnSmpCnt = 0;
       	 }
      // reset the sleep timeout counter if there is significant vertical movement
      if (ABS(gnCps) > SLEEP_THRESHOLD_CPS) {
         gnElapsedMin = 0;
         gnElapsedSec = 0;
         gnSecTick = 0;
 		 }
 	  else
      if (gnElapsedMin >= SLEEP_TIMEOUT_MINUTES) {
#ifdef DEBUG
         uart_Print("Timeout sleep\r\n");
#endif
         mcu_Sleep();
         }
              
      audio_Beep(gnCps);
#ifdef DEBUG      
      gpioDbg = 0;
#endif      
      gnSensorState = SENSOR_READ_TEMPERATURE;
      }
   }



/// Initialization of altitude sample buffer with average altitude
void sns_InitWindowBuffers(void) {
   s32 tc,tAccum,n,pa,pAccum;
   pAccum = 0;
   tAccum = 0;
   n = 8;
    while (n--) {
      sns_TriggerTemperatureSample();
      tmr1_BusyWaitMsx10(SENSOR_TEMPERATURE_TIMEOUT);
      gD2 = sns_ReadTemperatureSample();
      gTEMP = sns_CalculateTemperatureCx10();
      sns_TriggerPressureSample();
      tmr1_BusyWaitMsx10(SENSOR_PRESSURE_TIMEOUT);
      gD1 = sns_ReadPressureSample();
      pa = sns_CalculatePressurePa();
      pAccum += pa;
      tAccum += gTEMP;
      }
   tc = tAccum/8;
   gnTempC = (tc >= 0 ?  (tc+5)/10 : (tc-5)/10);
   gnPa = (pAccum+4)/8;
   gnAltAvgCm = sns_Pa2Cm(gnPa);
   gnAltAvgM = (gnAltAvgCm >= 0 ? (gnAltAvgCm+50L)/100L : (gnAltAvgCm-50L)/100L);
   n = gnNumSamples;
   while (n--) {
      gZBuf[n] = gnAltAvgCm;
      }
   gnSmpCnt = 0;
#ifdef DEBUG
   sprintf(gszBuf,"Tavg : %ld.%ldC\r\n",gnTempC/10,ABS(gnTempC)%10);
   uart_Print(gszBuf);
   sprintf(gszBuf,"Pavg : %ldPa\r\n",gnPa);
   uart_Print(gszBuf);
   sprintf(gszBuf,"Zavg : %ld.%02ldm\r\n",gnAltAvgCm/100,ABS(gnAltAvgCm)%100);
   uart_Print(gszBuf);
#endif
   }


void sns_AverageAltitude(void) {
   int cnt;
   s32 avg;
   cnt = gnNumSamples;
   avg = 0;
   while (cnt--)  {
	  avg += gZBuf[cnt];
      }
   avg = (avg >= 0 ? avg +gnNumSamples/2 : avg - gnNumSamples/2);
   gnAltAvgCm = avg/gnNumSamples;
   gnAltAvgM = ((gnAltAvgCm >= 0) ? (gnAltAvgCm+50L)/100L : (gnAltAvgCm-50L)/100L); 
   return; 
   }

#define MAX_CPS 2000L

/// Linear regression of altitude samples in buffer to calculate
/// vertical velocity. Calculation ok to +/- 20m/s with 32bit arithmetic
void sns_DeltaCmPerSec(void)   {
   int n,trel;
   s32 z, sum_zt,nCps;
   
   sum_zt = 0;
   n = gnNumSamples;
   while (n--)  {
      z = gZBuf[n] - gnAltAvgCm;
      trel = n - gnSmpCnt; // time origin is the current sample in window
      if (n > gnSmpCnt) {
         trel -= gnNumSamples;
         }
      sum_zt += ((s32)trel*z);
      }

   nCps = (sum_zt*(s32)(SENSOR_SAMPLES_PER_SEC*gnNumSamples))/gnDen;
   CLAMP(nCps,-MAX_CPS,MAX_CPS);
   gnAccel = (nCps - gnCps);
   sns_LPFAccel();
#ifdef DEBUG   
   //sprintf(gszBuf,"A %ld\r\n",gnAccel);
   //uart_Print(gszBuf);
#endif 
   gnCps = nCps;
   return;
   }

void sns_LPFAccel(void)   {
   	s32 tmp;
   	tmp = gnAccelx256*KLPF_ACCEL + (gnAccel<<8)*(256L-KLPF_ACCEL);
   	gnAccelx256 = (tmp >= 0 ? ((tmp + 128L)>>8) : ((tmp - 128L)>>8));
   	gnAccel = (gnAccelx256>>8);
   	}  

void sns_LPFClimbRate(void)   {
   	s32 tmp;
   	tmp = gnCpsx256*gnKLpf + (gnCps<<8)*(256L-gnKLpf);
   	gnCpsx256 = (tmp >= 0 ? ((tmp + 128L)>>8) : ((tmp - 128L)>>8));
   	gnCps = (gnCpsx256>>8);
   	}  


s32 sns_AdaptKLpf(void) {
	s32 klpf,nAccel;
	nAccel = ABS(gnAccel);
	CLAMP(nAccel, ACCEL_MIN_ADAPT, ACCEL_MAX_ADAPT);
	klpf = gnKLpfMax + ((KLPF_MIN_ADAPT-gnKLpfMax)*(nAccel - ACCEL_MIN_ADAPT))/(ACCEL_MAX_ADAPT-ACCEL_MIN_ADAPT);
	CLAMP(klpf, KLPF_MIN_ADAPT, KLPF_MAX_ADAPT);
	return klpf;
	}

/// Fast Lookup+Interpolation method for converting pressure readings to altitude readings.
#include "pztbl.txt"

s32 sns_Pa2Cm(s32 pa)  {
   	s32 inx,pa1,z1,z2,z;
    
   	if (pa > PA_INIT) {
      	z = gPZTbl[0];
      	}
   	else {
      	inx = (PA_INIT - pa)>>10;      
      	if (inx >= PZLUT_ENTRIES-1) {
         	z = gPZTbl[PZLUT_ENTRIES-1];
         	}
      	else {
         	pa1 = PA_INIT - (inx<<10);
         	z1 = gPZTbl[inx];
         	z2 = gPZTbl[inx+1];
         	z = z1 + (((pa1-pa)*(z2-z1))>>10);
         	}
      	}
   	return z;
   	}


///
/// Read the calibration coefficients from the BMP085 pressure sensor
void sns_ReadCoeffs(void) {
    u08 cnt;
    for (cnt = 0; cnt < 8; cnt++) {
    	CS = 0;
		spi_Send8(0xA0 + cnt*2);
		gC[cnt] = spi_Receive16();
		CS = 1;
		}
	}

void sns_Reset(void) {
    CS = 0;
	spi_Send8(0x1E);
	CS = 1;
	tmr1_BusyWaitMsx10(30);
	}

///
/// Trigger a pressure sample with max oversampling rate
void sns_TriggerPressureSample(void) {
    CS = 0;
	spi_Send8(0x48);
	CS = 1;
   }

///
/// Trigger a temperature sample with max oversampling rate
void sns_TriggerTemperatureSample(void) {
    CS = 0;
    spi_Send8(0x58);
    CS = 1;
   }

///
/// Read a raw temperature sample
u32 sns_ReadTemperatureSample(void)	{
   u32 w;
   CS = 0;
   spi_Send8(0x00);
   w = spi_Receive24();
   CS = 1;
   return w;
   }

///
/// Read a raw pressure sample
u32 sns_ReadPressureSample(void)	{
   u32 w;
   CS = 0;
   spi_Send8(0x00);
   w = spi_Receive24();
   CS = 1;
   return w;
   }

s32 sns_CalculateTemperatureCx10(void) {
	s32 t;
	gdT = (s32)gD2 - (((s32)gC[5])<<8);
	t = 2000 + ((gdT*((s32)gC[6]))>>23);
	return t;
	}

//#define MS5607
#define MS5611

s32 sns_CalculatePressurePa(void) {
	s32 pa;
#ifdef MS5611
	gOFF = (((s64)gC[2])<<16) + ((((s64)gC[4])*(s64)gdT)>>7);
	gSENS = (((s64)gC[1])<<15) + ((((s64)gC[3])*(s64)gdT)>>8);
	pa = (s32)((((((s64)gD1)*gSENS)>>21) - gOFF)>>15);
#endif
#ifdef MS5607
	gOFF = (((s64)gC[2])<<17) + ((((s64)gC[4])*(s64)gdT)>>6);
	gSENS = (((s64)gC[1])<<16) + ((((s64)gC[3])*(s64)gdT)>>7);
	pa = (s32)((((((s64)gD1)*gSENS)>>21) - gOFF)>>15);
#endif
	return pa;
	}

/// TMR3 used for 50mS clock time tick
#define FREQ_TMR3_HZ   20L

/// Elapsed time counter
void tmr3_Config(void){
	_T3IE = 0;  // disable interrupt
	T2CONbits.T32 = 0; // 16bit timer
    T3CONbits.TON = 0; // 16bit timer
    T3CONbits.TCS = 0; //  select internal clock 
    T3CONbits.TGATE = 0; // gate disabled
    T3CONbits.TSIDL = 0; // continue in idle mode
    T3CONbits.TCKPS1 = 0; // Prescaler 1:8
    T3CONbits.TCKPS0 = 1; // Prescaler 1:8
    PR3 = (u16)((FCY>>3)/FREQ_TMR3_HZ );
    gbTmr3Flag = 0;
    gnTimeTick = 0;
    gnSecTick = 0;
    gnElapsedSec = 0;
    gnElapsedMin = 0;
    gnElapsedHrs = 0;
    TMR3  = 0;   //zero timer3 counter
    _T3IF = 0;   //clear interrupt flag
    _T3IP = 1;   //choose a priority
	_T3IE = 1;  // enable interrupt
    T3CONbits.TON = 1; // start tmr3
	}

void __attribute__((interrupt, no_auto_psv)) _T3Interrupt(void) {
  	_T3IF = 0;    //clear the interrupt
   gbTmr3Flag = 1;
   gnTimeTick++;
   gnSecTick++;
   if (gnSecTick == FREQ_TMR3_HZ)  {
      gnSecTick = 0;
      gnElapsedSec++;
      if (gnElapsedSec == 60) {
         gnElapsedSec = 0;
         gnElapsedMin++;
         if (gnElapsedMin == 60)  {
            gnElapsedMin = 0;
            gnElapsedHrs++;
            }
         }      
      }
   }

///
/// TMR1 used for busy wait, flag wait, elapsed time count
void  tmr1_Config(void) {
   T1CONbits.TON = 0;    // turn off the timer
   T1CONbits.TCS = 0;    // select internal clock 
   T1CONbits.TGATE = 0;  // gate disabled
   T1CONbits.TSIDL = 0;  // continue in idle mode
   T1CONbits.TCKPS1 = 1; // Prescaler 1:256
   T1CONbits.TCKPS0 = 1; // Prescaler 1:256
   gbTmr1Flag = 0;
   PR1 = 65535;
   TMR1  = 0;            //clear timer1 value
   _T1IF = 0;            //clear interrupt flag
   _T1IP = 1;            //choose a priority
   _T1IE = 0;            //disable the interrupt
   }

void tmr1_Start(void) {
   	T1CONbits.TON = 0; //turn off the timer
   	TMR1  = 0;         //zero timer1 counter
   	T1CONbits.TON = 1; //turn on the timer
	}

int tmr1_ElapsedTimeMs(void) {
    int ms;
    _T1IE = 0;            //disable the interrupt
   	T1CONbits.TON = 0; //turn off the timer
    ms = (int)(((TMR1*1000L)<<8)/FCY);
    return ms;
	}

#define FCY_DIV_10000   25L

/// max delay is 67108 mS with FCY = 250kHz and prescaler=256
void tmr1_FlagWaitMsx10(u32 mSx10) {
    u16 cnt = (u16)((mSx10 * FCY_DIV_10000)>>8);
   	T1CONbits.TON = 0; // turn off timer
    gbTmr1Flag = 0;
   	TMR1 = 0;
    PR1  = cnt;         // set timer
   	T1CONbits.TON = 1; // turn on timer
    _T1IE = 1;            //enable the interrupt
	}    

void __attribute__((interrupt, no_auto_psv)) _T1Interrupt(void) {
   _T1IF = 0;    //clear the timer interrupt bit
   T1CONbits.TON = 0; // turn off timer
   _T1IE = 0;            //disable the interrupt   
   gbTmr1Flag = 1;
   }    

/// max delay is 67108 mS with FCY = 250kHz and prescaler=256
void tmr1_BusyWaitMsx10(u32 mSx10) {
    u16 cnt = (u16)((mSx10 * FCY_DIV_10000)>>8);
   	T1CONbits.TON = 0; // turn off timer
   	TMR1  = 0;         // zero timer1 counter
   	T1CONbits.TON = 1; // turn on timer
    while(TMR1 < cnt); // busy wait until interval completes
    T1CONbits.TON = 0; // turn off timer again
    return;
	}

#ifdef DEBUG

void uart_Config(void) {
   char tmp;
   U2BRG  = 6;     // 8928baud @ FCY = 250kHz, note that you must select "custom baud rate" in the
                   // pickit2 uart application drop-down window for baudrate, and enter the value 8928                   
   U2MODEbits.BRGH = 1;
   U2MODEbits.UARTEN = 1;
   U2MODEbits.RTSMD = 1; // no flow control
   U2MODEbits.PDSEL = 0; // 8 bits, no parity
   U2MODEbits.STSEL = 0; // 1 stop bit
   while(U2STAbits.URXDA == 1) {
      tmp = U2RXREG;
      }
   // clear IF flags 
   IFS1bits.U2RXIF = 0;
   IFS1bits.U2TXIF = 0;
   // set priority 
   IPC7bits.U2RXIP = 1;
   IPC7bits.U2TXIP = 1;
   // disable interrupt 
   IEC1bits.U2RXIE = 0;
   IEC1bits.U2TXIE = 0;
   // enable transmit
   U2STAbits.UTXEN = 1;
   }

void uart_Print(char *szBuf) {
   while(*szBuf) {
      U2TXREG = *szBuf++;   // transfer data byte to TX reg 
      while(!U2STAbits.TRMT);  // wait for xmit to complete
      }
   }


///
/// Calculate sensor pressure noise as variance of 64 consecutive pressure samples

#define NUM_NOISE_SAMPLES 64

s32 gPaTbl[NUM_NOISE_SAMPLES];

void sns_CalculateSensorNoisePa(void) {
   s32 n,pa,accum, pAvg,variance;
   s32 altitudeAverageCm;
   // one dummy sample of pressure and temperature
   sns_TriggerTemperatureSample();
   tmr1_BusyWaitMsx10(SENSOR_TEMPERATURE_TIMEOUT);
   gD2 = sns_ReadTemperatureSample();
   gTEMP = sns_CalculateTemperatureCx10();
   sns_TriggerPressureSample();
   tmr1_BusyWaitMsx10(SENSOR_PRESSURE_TIMEOUT);
   gD1 = sns_ReadPressureSample();
   // now start accumulating
   accum = 0;
   n = NUM_NOISE_SAMPLES;
   while (n--) {
      sns_TriggerTemperatureSample();
      tmr1_BusyWaitMsx10(SENSOR_TEMPERATURE_TIMEOUT);
      gD2 = sns_ReadTemperatureSample();
      gTEMP = sns_CalculateTemperatureCx10();
	  sns_TriggerPressureSample();
      tmr1_BusyWaitMsx10(SENSOR_PRESSURE_TIMEOUT);
      gD1 = sns_ReadPressureSample();
      gPaTbl[n] = pa = sns_CalculatePressurePa();
      accum += pa;
      }
   pAvg = (accum+NUM_NOISE_SAMPLES/2)/NUM_NOISE_SAMPLES;
   accum = 0;
   n = NUM_NOISE_SAMPLES;
   while (n--) {
	   accum += ((gPaTbl[n]-pAvg)*(gPaTbl[n]-pAvg));
	   }
   variance = (accum+NUM_NOISE_SAMPLES/2)/NUM_NOISE_SAMPLES;
   altitudeAverageCm = sns_Pa2Cm(pAvg);
   sprintf(gszBuf,"Variance %ld AltAvgCm %ld\r\n",variance, altitudeAverageCm);   
   uart_Print(gszBuf);
   }
#endif



///
/// Configure SPI hardware module for 8bit, mode 0, master
///
void spi_Config(void) {
	CS = 1;
	SCK = 0;
	}


void spi_Send8(u08 out) {
	u08 i = 0;
	for (i = 0; i < 8; i++)	{
		if (out & 0x80)	
			MOSI = 1;
		else
			MOSI = 0;
		SCK = 1;			
		out = (out << 1);	
		SCK = 0;			
	    }
    }


u16 spi_Receive16(void){
	u08 i,temp;
	u16 in;
	i = 0;
	in = 0;
	temp = 0;
	for (i = 0; i < 16; i++)	{
		in = (in << 1);		
		temp = (u08)MISO;	
		SCK = 1;		
	    if (temp == 1)	in = in | 0x0001;	
		SCK = 0;	
	    }
	return in;
    }

u32 spi_Receive24(void){
	u08 i,temp;
	u32 in;
	i = 0;
	in = 0;
	temp = 0;
	for (i = 0; i < 24; i++)	{
		in = (in << 1);	
		temp = (u08)MISO;
		SCK = 1;	
	    if (temp == 1)	in = in | 0x00000001;	
		SCK = 0;	
	    }
	return in;
    }


void adc_Config(u16  ch0PositiveMask, u08  autoSampleTime) {
  	CONFIG_AN0_AS_ANALOG();
  	if ( autoSampleTime > 31)  autoSampleTime=31;
  	AD1CON1bits.ADON = 0;   // turn off ADC (changing setting while ADON is not allowed)
  	AD1CON1 = ADC_CLK_AUTO + ADC_AUTO_SAMPLING_OFF;
  	AD1CON3 = ADC_CONV_CLK_INTERNAL_RC + ( autoSampleTime<<8);
  	AD1CON2 = ADC_VREF_AVDD_AVSS;
  	AD1CHS  = ADC_CH0_NEG_SAMPLEA_VREFN +  ch0PositiveMask;
	}


u16 adc_Convert(void) {
    u16 sample;
  	SET_SAMP_BIT_ADC1();                    //start sampling
  	WAIT_UNTIL_CONVERSION_COMPLETE_ADC1();  //wait for conversion to finish
    sample = ADC1BUF0;
  	return(sample);
	}
	
/// Indicate battery voltage with series of beeps

void bat_VoltageCheck(void) {
    int cnt,accum;	
    CONFIG_RB15_AS_DIG_OUTPUT();
    _LATB15 = 1; // forward bias the diode
    adc_Config(ADC_CH0_POS_SAMPLEA_AN0, 31);
  	AD1CON1bits.ADON = 1;   //turn on the ADC
  	accum = adc_Convert(); // junk the first sample
	cnt = 8;
    accum = 0;
    while (cnt--) { 
    	accum += (int)adc_Convert();
		}
  	AD1CON1bits.ADON = 0;   //turn off the ADC
	accum = (accum+4)/8; // round off average
#ifdef DEBUG	
    sprintf(gszBuf,"adc sample %d\r\n",accum);
    uart_Print(gszBuf);
#endif    
    if (accum <= 177) cnt = 5;
    else
    if (accum <= 190) cnt = 4;
    else
    if (accum <= 203) cnt = 3;
    else
    if (accum <= 216) cnt = 2;
    else
    if (accum <= 230) cnt = 1;
    else cnt = 0;
    // 5 tones for new battery, 
    // 1 tone for end of life battery
    // warning tone replace battery
    if (cnt == 0) {
        audio_SetFreq(2000);
        tmr1_BusyWaitMsx10(1000);
        audio_SetFreq(1000);
        tmr1_BusyWaitMsx10(1000);
        audio_SetFreq(500);
        tmr1_BusyWaitMsx10(1000);
        audio_SetFreq(250);
        tmr1_BusyWaitMsx10(1000);
        audio_SetFreq(0);
        }    
    while (cnt--) {
        audio_Tone(TONE_HI_FREQHZ,1000);
        tmr1_BusyWaitMsx10(2000);
        }    
    _LATB15 = 0; // stop driving the diode
    CONFIG_RA0_AS_DIG_INPUT();// reset RA0 to digital input
    _LATA0 = 0; 
#ifndef DEBUG    
    //CONFIG_RB15_AS_DIG_INPUT();// reset RB15 to digital input
#endif    
    }

